package com.lego.jbv;

import lejos.nxt.*;
import lejos.robotics.Color;

public class ColourSegway 
{

    // PID constants
    final int KP = 28;
    final int KI = 4;
    final int KD = 33;
    final int SCALE = 18;

    // Global vars:
    int offset;
    int prev_error;
    float int_error;
	
    static ColorSensor ls;
	
    public ColourSegway() 
    {
        ls = new ColorSensor(SensorPort.S2);
        ls.setFloodlight(Color.WHITE);
    }
    
    private static void init()
    {
    	Button.ESCAPE.addButtonListener(new ButtonListener() {
			
			@Override
			public void buttonReleased(Button b) {
				shutDown();
				LCD.clear();
			    System.exit(0);				
			}
			
			@Override
			public void buttonPressed(Button b) {
				// TODO Auto-generated method stub
				
			}
		});
    }
	
    public void getBalancePos() 
    {
    	LCD.clear();
    	LCD.drawString("Calibrating:", 0, 0);
    	
        // Wait for user to balance and press orange button
        while (!Button.ENTER.isDown())
        {
	        // NXTway must be balanced.
	        offset = ls.getNormalizedLightValue();
	        
	        LCD.drawInt(offset, 4, 10, 0);
        
	        
        }
    }
	
    public void pidControl() 
    {        
        while (true) 
        {
            int normVal = ls.getNormalizedLightValue();

            // Proportional Error:
            int error = normVal - offset;
            // Adjust far and near light readings:
            if (error < 0) error = (int)(error * 1.8F);
			
            // Integral Error:
            int_error = ((int_error + error) * 2)/3;
			
            // Derivative Error:
            int deriv_error = error - prev_error;
            prev_error = error;
			
            int pid_val = (int)(KP * error + KI * int_error + KD * deriv_error) / SCALE;
			
            if (pid_val > 100)
                pid_val = 100;
            if (pid_val < -100)
                pid_val = -100;

            // Power derived from PID value:
            int power = Math.abs(pid_val);
            power = 55 + (power * 45) / 100; // NORMALIZE POWER


            if (pid_val > 0) {
                MotorPort.B.controlMotor(power, BasicMotorPort.FORWARD);
                MotorPort.C.controlMotor(power, BasicMotorPort.FORWARD);
            } else {
                MotorPort.B.controlMotor(power, BasicMotorPort.BACKWARD);
                MotorPort.C.controlMotor(power, BasicMotorPort.BACKWARD);
            }
        }
    }
	
    public static void shutDown()
    {
        // Shut down light sensor, motors
        Motor.B.flt();
        Motor.C.flt();
        ls.setFloodlight(false);
    }
    
    private void sleep(int milliseconds)
    {
    	try {
			Thread.sleep(milliseconds);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    }
	
    public static void main(String[] args) 
    {
        ColourSegway sej = new ColourSegway();
        init();
        sej.getBalancePos();        
        sej.pidControl();
    }
}